#include "SensorDataTask.h"
#include <string.h>
#include <stdio.h>

volatile uint8_t uart_tx_complete = 1;

void UART_DMA_SendString(char *str) {
    uint16_t len = strlen(str);
    while (!uart_tx_complete) {
        osDelay(1);
    }
    uart_tx_complete = 0;
    HAL_UART_Transmit_DMA(&huart2, (uint8_t *) str, len);
}

void vSensorDataTask(void const *argument) {
    char buffer[100];
    SensorData_t sensorData;

    for (;;) {
//        taskENTER_CRITICAL();
        sensorData.csb = (int) distance;
//        taskEXIT_CRITICAL();

        sensorData.pv = 0.0f;
        sensorData.gs = 0;
        sensorData.lf = 0;
        sensorData.hw = 0;
        sensorData.gm = 0;

        snprintf(buffer, sizeof(buffer), "$4WD,CSB%d,PV%.1f,%d,%d,%d,%d#",
                 sensorData.csb, sensorData.pv, sensorData.gs, sensorData.lf, sensorData.hw, sensorData.gm);

        UART_DMA_SendString(buffer);

        osDelay(100);
    }
}
